/**
 * @file PathPlan.h
 * @author rj.wang (rj.wang@aotbot.com)
 * @brief 路径规划
 * @version 0.1
 * @date 2024-11-14
 * @copyright Copyright (c) 2024
 */

#ifndef __COMMON_PLANNING_PATH_PLAN_H_
#define __COMMON_PLANNING_PATH_PLAN_H_

#include <opencv2/opencv.hpp>

#include "GraphSearch.h"
#include "glog_func/log.h"

namespace common {
namespace planning {

/**
 * @brief 对start到goal的直线段路径, 按间距resolution进行采样/插值
 */
PoseDq InterpolatePoses(const Pose& start, 
                        const Pose& goal,
                        const double resolution);

/**
 * @brief 把result_info(PathPlan::ResultType)转换成字符串
 */
std::string ToStringResult(const uint8_t result_info);

class PathPlan {
public:
  PathPlan() {}
  ~PathPlan() {}

  /**
   * @brief 规划结果类型
   */
  enum ResultType {
    SUCCEED = 0,                   // 规划成功
    START_TRAPPED = 1,             // 规划失败 - 起点被困
    GOAL_TRAPPED = 2,              // 规划失败 - 目标点被困
    START_GOAL_NOT_CONNECTED = 3,  // 规划失败 - 起点和目标点不连通
    SEARCH_FAIL = 4,               // 规划失败 - 图搜索没有给出结果
  };

private:
  GraphSearch g_search_; // 栅格图搜索
  cv::Mat connected_{};  // 连通图
  
public:
  /**
   * @brief 初始化
   * @param binary_map 二值化的栅格地图（0-障碍物, 非0-空闲)
   * @param origin 栅格图左下角栅格中心对应的物理坐标
   * @param width 栅格图的宽, unit: 栅格
   * @param height 栅格图的高, unit: 栅格
   * @param resolution 栅格图的分辨率, unit: m
   * @param bump_dis 碰撞距离, unit: m (最近障碍物距离小于等于该值的栅格, 视为不可行区域)
   * @param safe_dis 安全距离, unit: m (最近障碍物距离大于该值的栅格, 视为足够安全的区域)
   */
  void Init(const std::vector<uint8_t>& binary_map,
            const Pose& origin, 
            const int64_t width,
            const int64_t height, 
            const double resolution,
            const double bump_dis,
            const double safe_dis);
  
  /**
   * @brief 便于访问距离场地图及使用相关方法
   */
  inline DistanceMap& DFMap() { return g_search_.DFMap(); }

  /**
   * @brief 搜索最小通行代价路径
   * @param start 起点, 物理坐标
   * @param goal 目标点, 物理坐标
   * @param resolution 搜索得到的路径点间距, unit: m
   * @param method 图搜算法(GraphSearch::DIJKSTRA, GraphSearch::ASTAR, GraphSearch::JPS)
   * @return PoseDq 路径, 物理坐标
   */
  PoseDq SearchPath(const Pose& start, 
                    const Pose& goal, 
                    const double resolution,
                    const uint8_t method = GraphSearch::ASTAR);

  /**
   * @brief pose对应的栅格是否被占用
   */
  bool IsOccupied(const Pose& pose);

  /**
   * @brief 在pose周围range(unit: m)范围内找到一个可行点, 更新pose为该可行点
   * @return true 更新成功
   * @return false range(unit: m)范围内无可行点
   */
  bool UpdateToFree(Pose& pose, const double range);

  /**
   * @brief 获取栅格(ix, iy)的连通图标记值
   */
  int32_t GetConnectedValue(const int64_t ix, const int64_t iy);

  /**
   * @brief 在pose周围range(unit: m)范围内找到一个连通图上标记为connected_value的可行点, 更新pose为该可行点
   * @return true 更新成功
   * @return false range(unit: m)范围内无符合条件的可行点
   */
  bool UpdateToConnected(Pose& pose, const double range, const int32_t connected_value);

  /**
   * @brief 检查p1和p2是否连通
   */
  bool IsConnected(const Pose& p1, const Pose& p2);

  /**
   * @brief 搜索最小通行代价路径
   * @param start 起点, 物理坐标
   * @param start_update_range 起点如果被占用, 允许在多大范围内找可行点更新起点, unit: m
   * @param goal 目标点, 物理坐标
   * @param goal_update_range 目标点如果被占用, 允许在多大范围内找可行点更新目标点, unit: m
   * @param resolution 搜索得到的路径点间距, unit: m
   * @param result_info 调用方可据此获取规划结果的详细信息 (取值范围: PathPlan::ResultType)
   * @param method 图搜算法(GraphSearch::DIJKSTRA, GraphSearch::ASTAR, GraphSearch::JPS)
   * @return PoseDq 路径, 物理坐标
   */
  PoseDq SearchPathWithResult(const Pose& start, 
                              const double start_update_range, 
                              const Pose& goal,
                              const double goal_update_range, 
                              const double resolution,
                              uint8_t& result_info, 
                              const uint8_t method = GraphSearch::ASTAR);
};

} // namespace planning
} // namespace common

#endif // __COMMON_PLANNING_PATH_PLAN_H_